#!/usr/bin/env python
import rospy
from std_msgs.msg import String  # 修改为接收字符串消息
import serial
import struct


class RobotHandINS:
    def __init__(self):
        self.port_name = "/dev/ttyUSB0"  # 串口名称
        self.baud_rate = 115200
        self.ser = None
        self.open_serial_port()

        # 初始化指头位置
        self.finger_positions = [1000, 1000, 1000, 1000, 1000, 350]  # 假设每个指头的角度

        # 初始化 ROS 节点并订阅控制话题
        rospy.init_node('hand_control_node', anonymous=True)
        self.control_subscriber = rospy.Subscriber('/hand_control_topic', String, self.control_hand_callback,
                                                   queue_size=1)

        # 定时器每隔1秒执行一次
        self.finger_index = 0  # 当前控制的指头索引
        self.is_reverse_order = False  # 控制顺序模式，False 为顺序，True 为逆序

        # 初始化定时器，默认周期为0.5秒
        self.timer_period = rospy.Duration(1)
        self.timer = rospy.Timer(self.timer_period, self.control_finger_periodically)

    def __del__(self):
        if self.ser:
            self.close_serial_port()

    def open_serial_port(self):
        try:
            self.ser = serial.Serial(self.port_name, self.baud_rate, timeout=1)
            rospy.loginfo(f"Serial port {self.port_name} opened successfully.")
        except Exception as e:
            rospy.logerr(f"Failed to open serial port {self.port_name}: {str(e)}")

    def close_serial_port(self):
        if self.ser:
            self.ser.close()
            rospy.loginfo(f"Serial port {self.port_name} closed.")

    def write_serial_port(self, data):
        try:
            self.ser.write(data)
            rospy.loginfo(f"Written data: {' '.join(f'{byte:02X}' for byte in data)}")
        except Exception as e:
            rospy.logerr(f"Error writing to serial port: {str(e)}")
            return False
        return True

    def calculate_checksum(self, hex_array):
        checksum = sum(hex_array[2:]) & 0xFF
        return checksum

    def write_single_finger_position_cmd(self, ifinger_name, ifinger_position):
        output = [
            0x55, 0xAA, 0x04, ifinger_name, 0x03, 0x37,
            ifinger_position & 0xFF, (ifinger_position >> 8) & 0xFF
        ]
        checksum = self.calculate_checksum(output)
        output.append(checksum)

        rospy.loginfo(f"Generated frame: {' '.join(f'{byte:02X}' for byte in output)}")

        return self.write_serial_port(bytearray(output))

    def control_finger_periodically(self, event):
        if 0 <= self.finger_index < len(self.finger_positions):
            finger_position = self.finger_positions[self.finger_index]
            rospy.loginfo(f"Controlling finger {self.finger_index + 1} with position {finger_position}")
            self.write_single_finger_position_cmd(self.finger_index + 1, finger_position)

            # 根据顺序模式更新指头索引
            if self.is_reverse_order:
                self.finger_index -= 1  # 逆序模式，索引递减
            else:
                self.finger_index += 1  # 顺序模式，索引递增
        else:
            # 所有指头已控制完毕，重置指头索引
            if self.is_reverse_order:
                self.finger_index = len(self.finger_positions) - 1  # 逆序模式，重置为最后一个指头
            else:
                self.finger_index = 0  # 顺序模式，重置为第一个指头

    def control_hand_callback(self, msg):
        command = msg.data
        if command == "paper_grasp":
            rospy.loginfo("Executing paper_grasp...")
            self.finger_positions = [520, 520, 500, 440, 750, 0]
            self.is_reverse_order = True  # 顺序模式
            
            #self.finger_index = len(self.finger_positions) - 1  # 重置为最后一个指头
            self.finger_index = 0  # 重置为第一个指头
            self.timer_period = rospy.Duration(1)  # 抓取速度为0.5秒
            self.timer.shutdown()  # 关闭当前定时器
            self.timer = rospy.Timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        elif command == "release":
            rospy.loginfo("Executing release...")
            self.finger_positions = [1000, 1000, 1000, 1000, 1000, 350]
            self.is_reverse_order = False  # 默认顺序模式
            self.finger_index = 0  # 重置为第一个指头
            self.timer_period = rospy.Duration(1)  # 释放速度为0.1秒
            self.timer.shutdown()  # 关闭当前定时器
            self.timer = rospy.Timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        elif command == "yogurt_grasp":
            rospy.loginfo("Executing yogurt_grasp...")
            self.finger_positions = [1200, 1200, 1200, 1200, 1900, 50]
            self.is_reverse_order = True  # 逆序模式
            self.finger_index = len(self.finger_positions) - 1  # 重置为最后一个指头
            self.timer_period = rospy.Duration(0.2)  # 抓取速度为0.5秒
            self.timer.shutdown()  # 关闭当前定时器
            self.timer = rospy.Timer(self.timer_period, self.control_finger_periodically)  # 重新启动定时器
        else:
            rospy.logwarn(f"Unknown command: {command}")
            return


if __name__ == '__main__':
    try:
        # 创建一个 RobotHandINS 实例，用于串口控制
        hand_control = RobotHandINS()
        rospy.spin()  # 启动 ROS 事件循环，等待消息回调
    except rospy.ROSInterruptException:
        pass